# /***********************************************
# *                                              *
# *    Jeff Molofee's Revised OpenGL Basecode    *
# *  Huge Thanks To Maxwell Sayles & Peter Puck  *
# *            http://nehe.gamedev.net           *
# *                     2001                     *
# *            Adapted to PyOpenGL GLUT          *
# *                                              *
# ***********************************************/
#
# NeHe Tutorial Lesson: 48 - ArcBall Rotation
#
# C version of tutorial by Terence J. Grant
# This code was created by Jeff Molofee 2000
# Ported to PyOpenGL 2.0 by Brian Leair 2004
#
# The port was based on the PyOpenGL tutorials and from
# PyOpenGLContext (tests/glprint.py)
#
# If you've found this code useful, feel free to let me know
# at (Brian Leair telcom_sage@yahoo.com).
#
# See original source and C based tutorial at http://nehe.gamedev.net
#
# Note:
# -----
# This code is not an ideal example of Pythonic coding or use of OO
# techniques. It is a simple and direct exposition of how to use the
# Open GL API in Python via the PyOpenGL package. It also uses GLUT,
# a high quality platform independent library. Due to using these APIs,
# this code is more like a C program using procedural programming.
#
# To run this example you will need:
# Python 	- www.python.org (v 2.3 as of 1/2004)
# PyOpenGL 	- pyopengl.sourceforge.net (v 2.0.1.07 as of 1/2004)
# Numeric Python	- (v.22 of "numpy" as of 1/2004) numpy.sourceforge.net
#
#

import copy
from math import cos, sin, sqrt

from OpenGL.GL import *
from OpenGL.GLUT import *
from OpenGL.GLU import *


try:
    import numpy as Numeric

    def sumDot(a, b):
        return Numeric.dot(a, b)
except ImportError:
    try:
        import Numeric

        def sumDot(a, b):
            return sum(Numeric.dot(a, b))
    except ImportError:
        print("This demo requires the numpy or Numeric extension, sorry")
        import sys

        sys.exit()


# *********************** Globals ***********************
# Python 2.2 defines these directly
"""
try:
    True
except NameError:
    True = 1==1
    False = 1==0
"""

# Some api in the chain is translating the keystrokes to this octal string
# so instead of saying: ESCAPE = 27, we use the following.
ESCAPE = b'\033'

# Number of the glut window.
window = 0

PI2 = 2.0 * 3.1415926535  # 2 * PI (not squared!) 		// PI Squared

# *********************** Globals ***********************
# Python 2.2 defines these directly
"""
try:
    True
except NameError:
    True = 1==1
    False = 1==0
"""


# //assuming IEEE-754(GLfloat), which i believe has max precision of 7 bits
Epsilon = 1.0e-5


class ArcBallT:
    def __init__(self, NewWidth, NewHeight):
        self.m_StVec = Vector3fT()
        self.m_EnVec = Vector3fT()
        self.m_AdjustWidth = 1.0
        self.m_AdjustHeight = 1.0
        self.setBounds(NewWidth, NewHeight)

    def __str__(self):
        str_rep = ""
        str_rep += "StVec = " + str(self.m_StVec)
        str_rep += "\nEnVec = " + str(self.m_EnVec)
        str_rep += "\n scale coords %f %f" % (self.m_AdjustWidth, self.m_AdjustHeight)
        return str_rep

    def setBounds(self, NewWidth, NewHeight):
        # //Set new bounds
        assert (NewWidth > 1.0 and NewHeight > 1.0), "Invalid width or height for bounds."
        # //Set adjustment factor for width/height
        self.m_AdjustWidth = 1.0 / ((NewWidth - 1.0) * 0.5)
        self.m_AdjustHeight = 1.0 / ((NewHeight - 1.0) * 0.5)

    def _mapToSphere(self, NewPt):
        # Given a new window coordinate, will modify NewVec in place
        X = 0
        Y = 1
        Z = 2

        NewVec = Vector3fT()
        # //Copy paramter into temp point
        TempPt = copy.copy(NewPt)
        print('NewPt', NewPt, TempPt)
        # //Adjust point coords and scale down to range of [-1 ... 1]
        TempPt[X] = (NewPt[X] * self.m_AdjustWidth) - 1.0
        TempPt[Y] = 1.0 - (NewPt[Y] * self.m_AdjustHeight)
        # //Compute the square of the length of the vector to the point from the center
        length = sumDot(TempPt, TempPt)
        # //If the point is mapped outside of the sphere... (length > radius squared)
        if (length > 1.0):
            # //Compute a normalizing factor (radius / sqrt(length))
            norm = 1.0 / sqrt(length)

            # //Return the "normalized" vector, a point on the sphere
            NewVec[X] = TempPt[X] * norm
            NewVec[Y] = TempPt[Y] * norm
            NewVec[Z] = 0.0
        else:  # //Else it's on the inside
            # //Return a vector to a point mapped inside the sphere sqrt(radius squared - length)
            NewVec[X] = TempPt[X]
            NewVec[Y] = TempPt[Y]
            NewVec[Z] = sqrt(1.0 - length)

        return NewVec

    def click(self, NewPt):
        # //Mouse down (Point2fT
        self.m_StVec = self._mapToSphere(NewPt)
        return

    def drag(self, NewPt):
        # //Mouse drag, calculate rotation (Point2fT Quat4fT)
        """ drag (Point2fT mouse_coord) -> new_quaternion_rotation_vec
        """
        X = 0
        Y = 1
        Z = 2
        W = 3

        self.m_EnVec = self._mapToSphere(NewPt)

        # //Compute the vector perpendicular to the begin and end vectors
        # Perp = Vector3fT ()
        Perp = Vector3fCross(self.m_StVec, self.m_EnVec)

        NewRot = Quat4fT()
        # //Compute the length of the perpendicular vector
        if (Vector3fLength(Perp) > Epsilon):  # //if its non-zero
            # //We're ok, so return the perpendicular vector as the transform after all
            NewRot[X] = Perp[X]
            NewRot[Y] = Perp[Y]
            NewRot[Z] = Perp[Z]
            # //In the quaternion values, w is cosine (theta / 2), where theta is rotation angle
            NewRot[W] = Vector3fDot(self.m_StVec, self.m_EnVec)
        else:  # //if its zero
            # //The begin and end vectors coincide, so return a quaternion of zero matrix (no rotation)
            NewRot.X = NewRot.Y = NewRot.Z = NewRot.W = 0.0

        return NewRot


# ##################### Math utility ##########################################


def Matrix4fT():
    return Numeric.identity(4, 'f')


def Matrix3fT():
    return Numeric.identity(3, 'f')


def Quat4fT():
    return Numeric.zeros(4, 'f')


def Vector3fT():
    return Numeric.zeros(3, 'f')


def Point2fT(x=0.0, y=0.0):
    pt = Numeric.zeros(2, 'f')
    pt[0] = x
    pt[1] = y
    return pt


def Vector3fDot(u, v):
    # Dot product of two 3f vectors
    dotprod = Numeric.dot(u, v)
    return dotprod


def Vector3fCross(u, v):
    # Cross product of two 3f vectors
    X = 0
    Y = 1
    Z = 2
    cross = Numeric.zeros(3, 'f')
    cross[X] = (u[Y] * v[Z]) - (u[Z] * v[Y])
    cross[Y] = (u[Z] * v[X]) - (u[X] * v[Z])
    cross[Z] = (u[X] * v[Y]) - (u[Y] * v[X])
    return cross


def Vector3fLength(u):
    mag_squared = sumDot(u, u)
    mag = sqrt(mag_squared)
    return mag


def Matrix3fSetIdentity():
    return Numeric.identity(3, 'f')


def Matrix3fMulMatrix3f(matrix_a, matrix_b):
    return sumDot(matrix_a, matrix_b)


def Matrix4fSVD(NewObj):
    X = 0
    Y = 1
    Z = 2
    s = sqrt(((NewObj[X][X] * NewObj[X][X]) + (NewObj[X][Y] * NewObj[X][Y]) + (NewObj[X][Z] * NewObj[X][Z]) + (
        NewObj[Y][X] * NewObj[Y][X]) + (NewObj[Y][Y] * NewObj[Y][Y]) + (NewObj[Y][Z] * NewObj[Y][Z]) + (
                  NewObj[Z][X] * NewObj[Z][X]) + (NewObj[Z][Y] * NewObj[Z][Y]) + (NewObj[Z][Z] * NewObj[Z][Z]) ) / 3.0)
    return s


def Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix):
    # Modifies NewObj in-place by replacing its upper 3x3 portion from the
    # passed in 3x3 matrix.
    # NewObj = Matrix4fT ()
    NewObj[0:3, 0:3] = three_by_three_matrix
    return NewObj


# /**
# * Sets the rotational component (upper 3x3) of this matrix to the matrix
# * values in the T precision Matrix3d argument the other elements of
# * this matrix are unchanged a singular value decomposition is performed
# * on this object's upper 3x3 matrix to factor out the scale, then this
# * object's upper 3x3 matrix components are replaced by the passed rotation
# * components, and then the scale is reapplied to the rotational
# * components.
# * @param three_by_three_matrix T precision 3x3 matrix
# */
def Matrix4fSetRotationFromMatrix3f(NewObj, three_by_three_matrix):
    scale = Matrix4fSVD(NewObj)

    NewObj = Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix)
    scaled_NewObj = NewObj * scale  # Matrix4fMulRotationScale(NewObj, scale)
    return scaled_NewObj


def Matrix3fSetRotationFromQuat4f(q1):
    # Converts the H quaternion q1 into a new equivalent 3x3 rotation matrix.
    X = 0
    Y = 1
    Z = 2
    W = 3

    NewObj = Matrix3fT()
    n = sumDot(q1, q1)
    s = 0.0
    if (n > 0.0):
        s = 2.0 / n
    xs = q1[X] * s
    ys = q1[Y] * s
    zs = q1[Z] * s
    wx = q1[W] * xs
    wy = q1[W] * ys
    wz = q1[W] * zs
    xx = q1[X] * xs
    xy = q1[X] * ys
    xz = q1[X] * zs
    yy = q1[Y] * ys
    yz = q1[Y] * zs
    zz = q1[Z] * zs
    # This math all comes about by way of algebra, complex math, and trig identities.
    # See Lengyel pages 88-92
    NewObj[X][X] = 1.0 - (yy + zz)
    NewObj[Y][X] = xy - wz
    NewObj[Z][X] = xz + wy
    NewObj[X][Y] = xy + wz
    NewObj[Y][Y] = 1.0 - (xx + zz)
    NewObj[Z][Y] = yz - wx
    NewObj[X][Z] = xz - wy
    NewObj[Y][Z] = yz + wx
    NewObj[Z][Z] = 1.0 - (xx + yy)

    return NewObj


def unit_test_ArcBall_module():
    # Unit testing of the ArcBall calss and the real math behind it.
    # Simulates a click and drag followed by another click and drag.
    print("unit testing ArcBall")
    Transform = Matrix4fT()
    LastRot = Matrix3fT()
    ThisRot = Matrix3fT()

    ArcBall = ArcBallT(640, 480)
    # print "The ArcBall with NO click"
    # print ArcBall
    # First click
    LastRot = copy.copy(ThisRot)
    mouse_pt = Point2fT(500, 250)
    ArcBall.click(mouse_pt)
    # print "The ArcBall with first click"
    # print ArcBall
    # First drag
    mouse_pt = Point2fT(475, 275)
    ThisQuat = ArcBall.drag(mouse_pt)
    # print "The ArcBall after first drag"
    # print ArcBall
    # print
    # print
    print("Quat for first drag")
    print(ThisQuat)
    ThisRot = Matrix3fSetRotationFromQuat4f(ThisQuat)
    # Linear Algebra matrix multiplication A = old, B = New : C = A * B
    ThisRot = Matrix3fMulMatrix3f(LastRot, ThisRot)
    Transform = Matrix4fSetRotationFromMatrix3f(Transform, ThisRot)
    print("First transform")
    print(Transform)
    # Done with first drag


    # second click
    LastRot = copy.copy(ThisRot)
    print("LastRot at end of first drag")
    print(LastRot)
    mouse_pt = Point2fT(350, 260)
    ArcBall.click(mouse_pt)
    # second drag
    mouse_pt = Point2fT(450, 260)
    ThisQuat = ArcBall.drag(mouse_pt)
    # print "The ArcBall"
    # print ArcBall
    print("Quat for second drag")
    print(ThisQuat)
    ThisRot = Matrix3fSetRotationFromQuat4f(ThisQuat)
    ThisRot = Matrix3fMulMatrix3f(LastRot, ThisRot)
    # print ThisRot
    Transform = Matrix4fSetRotationFromMatrix3f(Transform, ThisRot)
    print("Second transform")
    print(Transform)
    # Done with second drag
    LastRot = copy.copy(ThisRot)


g_Transform = Matrix4fT()
g_LastRot = Matrix3fT()
g_ThisRot = Matrix3fT()

g_ArcBall = ArcBallT(640, 480)
g_isDragging = False
g_quadratic = None


# A general OpenGL initialization function.  Sets all of the initial parameters.
def Initialize(Width, Height):  # We call this right after our OpenGL window is created.
    global g_quadratic

    glClearColor(0.0, 0.0, 0.0, 1.0)  # This Will Clear The Background Color To Black
    glClearDepth(1.0)  # Enables Clearing Of The Depth Buffer
    glDepthFunc(GL_LEQUAL)  # The Type Of Depth Test To Do
    glEnable(GL_DEPTH_TEST)  # Enables Depth Testing
    glShadeModel(GL_FLAT)  # Select Flat Shading (Nice Definition Of Objects)
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)  # Really Nice Perspective Calculations

    g_quadratic = gluNewQuadric()
    gluQuadricNormals(g_quadratic, GLU_SMOOTH)
    gluQuadricDrawStyle(g_quadratic, GLU_FILL)
    # Why? this tutorial never maps any textures?! ?
    # gluQuadricTexture(g_quadratic, GL_TRUE)			# // Create Texture Coords

    glEnable(GL_LIGHT0)
    glEnable(GL_LIGHTING)

    glEnable(GL_COLOR_MATERIAL)

    return True


def Upon_Drag(cursor_x, cursor_y):
    """ Mouse cursor is moving
        Glut calls this function (when mouse button is down)
        and pases the mouse cursor postion in window coords as the mouse moves.
    """
    global g_isDragging, g_LastRot, g_Transform, g_ThisRot

    if g_isDragging:
        mouse_pt = Point2fT(cursor_x, cursor_y)
        ThisQuat = g_ArcBall.drag(mouse_pt)  # // Update End Vector And Get Rotation As Quaternion
        g_ThisRot = Matrix3fSetRotationFromQuat4f(ThisQuat)  # // Convert Quaternion Into Matrix3fT
        # Use correct Linear Algebra matrix multiplication C = A * B
        g_ThisRot = Matrix3fMulMatrix3f(g_LastRot, g_ThisRot)  # // Accumulate Last Rotation Into This One
        g_Transform = Matrix4fSetRotationFromMatrix3f(g_Transform,
                                                      g_ThisRot)  # // Set Our Final Transform's Rotation From This One
    return


def Upon_Click(button, button_state, cursor_x, cursor_y):
    """ Mouse button clicked.
        Glut calls this function when a mouse button is
        clicked or released.
    """
    global g_isDragging, g_LastRot, g_Transform, g_ThisRot

    g_isDragging = False
    if button == GLUT_RIGHT_BUTTON and button_state == GLUT_UP:
        # Right button click
        g_LastRot = Matrix3fSetIdentity()  # // Reset Rotation
        g_ThisRot = Matrix3fSetIdentity()  # // Reset Rotation
        g_Transform = Matrix4fSetRotationFromMatrix3f(g_Transform, g_ThisRot)  # // Reset Rotation
    elif button == GLUT_LEFT_BUTTON and button_state == GLUT_UP:
        # Left button released
        g_LastRot = copy.copy(g_ThisRot)  # // Set Last Static Rotation To Last Dynamic One
    elif button == GLUT_LEFT_BUTTON and button_state == GLUT_DOWN:
        # Left button clicked down
        g_LastRot = copy.copy(g_ThisRot)  # // Set Last Static Rotation To Last Dynamic One
        g_isDragging = True  # // Prepare For Dragging
        mouse_pt = Point2fT(cursor_x, cursor_y)
        g_ArcBall.click(mouse_pt)  # // Update Start Vector And Prepare For Dragging

    return


def Torus(MinorRadius, MajorRadius):
    # // Draw A Torus With Normals
    glBegin(GL_TRIANGLE_STRIP)  # // Start A Triangle Strip
    for i in range(20):  # // Stacks
        for j in range(-1, 20):  # // Slices
            # NOTE, python's definition of modulus for negative numbers returns
            # results different than C's
            # (a / d)*d  +  a % d = a
            if j < 0:
                wrapFrac = (-j % 20) / 20.0
                wrapFrac *= -1.0
            else:
                wrapFrac = (j % 20) / 20.0
            phi = PI2 * wrapFrac
            sinphi = sin(phi)
            cosphi = cos(phi)

            r = MajorRadius + MinorRadius * cosphi

            glNormal3f(sin(PI2 * (i % 20 + wrapFrac) / 20.0) * cosphi, sinphi,
                       cos(PI2 * (i % 20 + wrapFrac) / 20.0) * cosphi)
            glVertex3f(sin(PI2 * (i % 20 + wrapFrac) / 20.0) * r, MinorRadius * sinphi,
                       cos(PI2 * (i % 20 + wrapFrac) / 20.0) * r)

            glNormal3f(sin(PI2 * (i + 1 % 20 + wrapFrac) / 20.0) * cosphi, sinphi,
                       cos(PI2 * (i + 1 % 20 + wrapFrac) / 20.0) * cosphi)
            glVertex3f(sin(PI2 * (i + 1 % 20 + wrapFrac) / 20.0) * r, MinorRadius * sinphi,
                       cos(PI2 * (i + 1 % 20 + wrapFrac) / 20.0) * r)
    glEnd()  # // Done Torus
    return


def Draw():
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)  # // Clear Screen And Depth Buffer
    glLoadIdentity()  # // Reset The Current Modelview Matrix
    glTranslatef(-1.5, 0.0, -6.0)  # // Move Left 1.5 Units And Into The Screen 6.0

    glPushMatrix()  # // NEW: Prepare Dynamic Transform
    glMultMatrixf(g_Transform)  # // NEW: Apply Dynamic Transform
    glColor3f(0.75, 0.75, 1.0)
    Torus(0.30, 1.00)
    glPopMatrix()  # // NEW: Unapply Dynamic Transform

    glLoadIdentity()  # // Reset The Current Modelview Matrix
    glTranslatef(1.5, 0.0, -6.0)  # // Move Right 1.5 Units And Into The Screen 7.0

    glPushMatrix()  # // NEW: Prepare Dynamic Transform
    glMultMatrixf(g_Transform)  # // NEW: Apply Dynamic Transform
    glColor3f(1.0, 0.75, 0.75)
    gluSphere(g_quadratic, 1.3, 20, 20)
    glPopMatrix()  # // NEW: Unapply Dynamic Transform

    glFlush()  # // Flush The GL Rendering Pipeline
    glutSwapBuffers()
    return


# A general OpenGL initialization function.  Sets all of the initial parameters.
def InitGL(Width, Height):  # We call this right after our OpenGL window is created.

    glShadeModel(GL_SMOOTH)  # Enables Smooth Color Shading
    glClearColor(0.0, 0.0, 0.0, 0.5)  # This Will Clear The Background Color To Black
    glClearDepth(1.0)  # Enables Clearing Of The Depth Buffer
    glEnable(GL_DEPTH_TEST)  # Enables Depth Testing
    glDepthFunc(GL_LEQUAL)  # The Type Of Depth Test To Do
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)  # Really Nice Perspective Calculations

    return True  # // Initialization Went OK


# Reshape The Window When It's Moved Or Resized
def ReSizeGLScene(Width, Height):
    if Height == 0:  # Prevent A Divide By Zero If The Window Is Too Small
        Height = 1

    glViewport(0, 0, Width, Height)  # Reset The Current Viewport And Perspective Transformation
    glMatrixMode(GL_PROJECTION)  # // Select The Projection Matrix
    glLoadIdentity()  # // Reset The Projection Matrix
    # // field of view, aspect ratio, near and far
    # This will squash and stretch our objects as the window is resized.
    # Note that the near clip plane is 1 (hither) and the far plane is 1000 (yon)
    gluPerspective(45.0, float(Width) / float(Height), 1, 100.0)

    glMatrixMode(GL_MODELVIEW)  # // Select The Modelview Matrix
    glLoadIdentity()  # // Reset The Modelview Matrix
    g_ArcBall.setBounds(Width, Height)  # //*NEW* Update mouse bounds for arcball
    return


# The function called whenever a key is pressed. Note the use of Python tuples to pass in: (key, x, y)
def keyPressed(*args):
    global g_quadratic
    # If escape is pressed, kill everything.
    key = args[0]
    if key == ESCAPE:
        gluDeleteQuadric(g_quadratic)
        sys.exit()


def main():
    global window
    # pass arguments to init
    glutInit(sys.argv)

    # Select type of Display mode:
    # Double buffer
    # RGBA color
    # Alpha components supported
    # Depth buffer
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH)

    # get a 640 x 480 window
    glutInitWindowSize(640, 480)

    # the window starts at the upper left corner of the screen
    glutInitWindowPosition(0, 0)

    # Okay, like the C version we retain the window id to use when closing, but for those of you new
    # to Python, remember this assignment would make the variable local and not global
    # if it weren't for the global declaration at the start of main.
    window = glutCreateWindow("Lesson 48: NeHe ArcBall Rotation Tutorial")

    # Register the drawing function with glut, BUT in Python land, at least using PyOpenGL, we need to
    # set the function pointer and invoke a function to actually register the callback, otherwise it
    # would be very much like the C version of the code.
    glutDisplayFunc(Draw)

    # Uncomment this line to get full screen.
    # glutFullScreen()

    # When we are doing nothing, redraw the scene.
    glutIdleFunc(Draw)

    # Register the function called when our window is resized.
    glutReshapeFunc(ReSizeGLScene)

    # Register the function called when the keyboard is pressed.
    glutKeyboardFunc(keyPressed)

    # GLUT When mouse buttons are clicked in window
    glutMouseFunc(Upon_Click)

    # GLUT When the mouse mvoes
    glutMotionFunc(Upon_Drag)

    # We've told Glut the type of window we want, and we've told glut about
    # various functions that we want invoked (idle, resizing, keyboard events).
    # Glut has done the hard work of building up thw windows DC context and
    # tying in a rendering context, so we are ready to start making immediate mode
    # GL calls.
    # Call to perform inital GL setup (the clear colors, enabling modes
    Initialize(640, 480)

    # Start Event Processing Engine
    glutMainLoop()

# Print message to console, and kick off the main to get it rolling.
if __name__ == "__main__":
    print("Hit ESC key to quit.")
    main()

